#include "fonctionsInterruptibles.h"

volatile float Kp = 1;
volatile float Ki = 0.5;
volatile float Kd = 0.01;

volatile long unsigned temps, tempsInitial = 0; 
volatile float vitesseInterruptible = 0;
volatile float inclinaisonInterruptible = 0;

volatile float erreur = 0;
volatile float erreurOld = 0;
volatile double integral = 0;
volatile double derivee = 0;
volatile float proportionnel = 0;
volatile float vitesseAsservie;

volatile float vitesseVoulueInterruptible;
volatile float inclinaisonVoulueInterruptible;
int choix = 0;

void tapisInterruptible(){
  Serial.println("Modifier l'inclinaison (1) ou la vitesse (2)"); //Affichage de cette chaîne de caractère
  while(1){  //Boucle infini
    if(Serial.available() >=1 ){  //Si l'arduino a reçu des données depuis le port USB
      if(choix == 2){
        vitesseVoulueInterruptible = Serial.parseFloat(); //Récupération de la vitesse
        Serial.println(vitesseVoulueInterruptible);
        vitesseAsservie = vitesseVoulueInterruptible;
        tempsInitial = micros();
        integral = 0;
        derivee = 0;
        proportionnel = 0;
        erreur = 0;
        choix = 0;
      }
      else if(choix == 1){
        inclinaisonVoulueInterruptible = Serial.parseFloat();
        Serial.println(inclinaisonVoulueInterruptible);  //Récupération de l'inclinaison
        commandeInclinaisonInterruptible(inclinaisonVoulueInterruptible);  //Lancement de la fonction de commande
        choix = 0;
      }
      else{
        choix = Serial.parseInt();  //Récupération du choix
        Serial.println("Entrer la valeur voulue (0-22 km/h et 0-10 %)");
      }
    }
    commandeVitesseInterruptible(vitesseAsservie);
  }
}

void lectureVitesseInterruptible(){
  temps = micros() - tempsInitial;
  tempsInitial = micros();
  erreurOld = erreur;
  vitesseInterruptible = 3.6 * 0.135 * 1000000 / temps; //Conversion d'après V=D/T
  Serial.print("Vitesse reel : "); //Affichage de cette chaîne de caractère
  Serial.println(vitesseInterruptible);
  erreur = vitesseVoulueInterruptible - vitesseInterruptible;
  proportionnel = erreur;
  integral = integral + (erreur+erreurOld)*temps/(2*1000000);
  derivee = (erreur - erreurOld)*1000000/temps;
  vitesseAsservie = Kp*proportionnel + Ki*integral + Kd*derivee;
  //Serial.println("Modifier l'inclinaison (1) ou la vitesse (2)"); //Affichage de cette chaîne de caractère
}

void lectureInclinaisonInterruptible(){
  if(digitalRead(pinCommandeInclinaisonMontee) > 0 && digitalRead(pinCommandeInclinaisonDescente) == 0){  //Si le tapis monte
    inclinaisonInterruptible = inclinaisonInterruptible + 0.125;  //Le compteur inclinaison augmente
  }
  else if(digitalRead(pinCommandeInclinaisonMontee) == 0 && digitalRead(pinCommandeInclinaisonDescente) > 0){  //Si le tapis descend
    inclinaisonInterruptible = inclinaisonInterruptible - 0.125;  //Le compteur inclinaison diminue
  }
  commandeInclinaisonInterruptible(inclinaisonVoulueInterruptible);  //Execution de la fonction commande inclinaison pour savoir s'il faut encore le descendre
  Serial.print("Inclinaison reel : ");
  Serial.println(inclinaisonInterruptible);
  //Serial.println("Modifier l'inclinaison (1) ou la vitesse (2)"); //Affichage de cette chaîne de caractère
}

void commandeInclinaisonInterruptible(float inclinaison){
  if(inclinaison<inclinaisonInterruptible){
    digitalWrite(pinCommandeInclinaisonDescente, HIGH); //On descend le tapis en mettant le pin correspondant à l'état haut
  }
  else if(inclinaison>inclinaisonInterruptible){
    digitalWrite(pinCommandeInclinaisonMontee, HIGH); //On descend le tapis en mettant le pin correspondant à l'état haut
  }
  else{
    digitalWrite(pinCommandeInclinaisonDescente, LOW); //On descend le tapis en mettant le pin correspondant à l'état haut
    digitalWrite(pinCommandeInclinaisonMontee, LOW); //On descend le tapis en mettant le pin correspondant à l'état haut
  }
}

void commandeVitesseInterruptible(float vitesse){ //Fonction commandeVitesse, permet de régler la vitesse du tapis
  if(vitesse >0 && vitesse <= 22){
    unsigned int cycle = 1000*(vitesse*1.9 + 5.3); //équation reliant la vitesse du tapis et le cycle (tON) du signal (c.f. étude signal de commande vitesse) à mieux déterminer !
    pwmInterruptible(pinCommandeVitesse, 50000, cycle); //Lance la fonction PWM(pin, periode, cycle)
  }
}

void pwmInterruptible(int pin, int periode, unsigned int cycle){  //Fonction PWM implémenté par mes soins
  //Algorithme de la fonction pour créer une période élémentaire
  digitalWrite(pin, HIGH); //état haut
  delay(cycle*0.001);  //tON en ms
  delayMicroseconds(cycle%1000);
  digitalWrite(pin, LOW);  //état bas
  delay((periode-cycle)*0.001);  //tOFF en ms
  delayMicroseconds((periode-cycle)%1000);
}

void tapisTEST(int choixTEST){
  if(choixTEST == 1){
    commandeVitesseInterruptible(4);
  }
  else{
    commandeInclinaisonInterruptible(2);
  }
}
